#include "AStar/AStarWorldVector3.h"
#include "Subsystems/SubsystemTerrain.h"
#include "GameSingleton.h"
#include "Managers/BlocksManager.h"

#include "Blocks/DoorBlock.h"
#include "Blocks/FenceGateBlock.h"

namespace PocketSurvival
{

    AStarWorldVector3::AStarWorldVector3()
    {
        terrain = GameSingleton::singletonPtr->getSingleton<SubsystemTerrain>();
    }

    void AStarWorldVector3::getNeighbors(const Vector3 &p, std::vector<Vector3> &neighbors)
    {
        neighbors.clear();
        addNeighbor(neighbors, p, 1, 0);
        addNeighbor(neighbors, p, -1, 0);
        addNeighbor(neighbors, p, 0, -1);
        addNeighbor(neighbors, p, 0, 1);
        addNeighbor(neighbors, p, -1, -1);
        addNeighbor(neighbors, p, 1, -1);
        addNeighbor(neighbors, p, 1, 1);
        addNeighbor(neighbors, p, -1, 1);
    }

    void AStarWorldVector3::addNeighbor(std::vector<Vector3> &neighbors, const Vector3 &p, int32_t dx, int32_t dz)
    {
        float y = p.posY;
        float num = p.posY;
        int32_t num2 = Terrain::ToCell(p.posX) + dx;
        int32_t num3 = Terrain::ToCell(p.posY);
        int32_t num4 = Terrain::ToCell(p.posZ) + dz;
        int32_t cellValue = terrain->getCellValue(num2, num3, num4);
        int32_t num5 = Terrain::ExtractContents(cellValue);

        Block *block = GameSingleton::blocksManager->blocks[num5];
        if (block->shouldAvoid(cellValue))
        {
            return;
        }
        if (block->IsCollidable)
        {
            float blockWalkingHeight = getBlockWalkingHeight(block, cellValue);
            if (blockWalkingHeight > 0.5f && (block->NoAutoJump || block->NoSmoothRise))
            {
                return;
            }
            y = num3 + blockWalkingHeight;
            num = num3 + blockWalkingHeight;
        }
        else
        {
            bool flag = false;
            for (int32_t num6 = -1; num6 >= -4; num6--)
            {
                int32_t cellValue2 = terrain->getCellValue(num2, num3 + num6, num4);
                int32_t num7 = Terrain::ExtractContents(cellValue2);
                Block *block2 = GameSingleton::blocksManager->blocks[num7];
                if (block2->shouldAvoid(cellValue2))
                {
                    return;
                }
                if (block2->IsCollidable)
                {
                    y = num3 + num6 + 1;
                    flag = true;
                    break;
                }
            }
            if (!flag)
            {
                return;
            }
        }

        int32_t num8 = (dx == 0 || dz == 0) ? 2 : 3;
        Vector3 vector(p.posX, num + 0.01f, p.posZ);
        Vector3 v(num2 + 0.5f, num + 0.01f, num4 + 0.5f);
        Vector3 v2 = 1.0f / num8 * (v - vector);
        for (int32_t i = 1; i <= num8; i++)
        {
            Vector3 v3 = vector + i * v2;

            Vector3 min = v3 - Vector3(request.BoxSize.posX / 2.0f + 0.01f, 0.0f, request.BoxSize.posZ / 2.0f + 0.01f);
            Vector3 max = v3 + Vector3(request.BoxSize.posX / 2.0f - 0.01f, request.BoxSize.posY, request.BoxSize.posZ / 2.0f - 0.01f);

            BoundingBox box(min, max);
            if (isBlocked(box))
            {
                return;
            }
        }

        neighbors.emplace_back(num2 + 0.5f, y, num4 + 0.5f);
    }

    float AStarWorldVector3::getBlockWalkingHeight(Block *block, int32_t value)
    {
        if (block->isBlock<DoorBlock *>() || block->isBlock<FenceGateBlock>())
        {
            return 0.0f;
        }
        float num = 0.0f;

        const std::vector<BoundingBox> &customCollisionBoxes = block->getCustomCollisionBoxes(value);
        for (const auto &boundingBox : customCollisionBoxes)
        {
            num = MathUtils::Max(num, boundingBox.Max.posY);
        }
        return num;
    }

    bool AStarWorldVector3::isBlocked(const BoundingBox &box)
    {
        int32_t num = Terrain::ToCell(box.Min.posX);
        int32_t num2 = MathUtils::Max(Terrain::ToCell(box.Min.posY), 0);
        int32_t num3 = Terrain::ToCell(box.Min.posZ);
        int32_t num4 = Terrain::ToCell(box.Max.posX);
        int32_t num5 = MathUtils::Min(Terrain::ToCell(box.Max.posY), 255);
        int32_t num6 = Terrain::ToCell(box.Max.posZ);
        for (int32_t i = num; i <= num4; i++)
        {
            for (int32_t j = num3; j <= num6; j++)
            {
                TerrainChunk *chunkAtCell = terrain->getChunkAtCell(i, j);
                if (chunkAtCell == nullptr)
                {
                    continue;
                }
                int32_t num7 = TerrainChunk::CalculateCellIndex(i & 0xF, num2, j & 0xF);
                int32_t num8 = num2;
                while (num8 <= num5)
                {
                    int32_t cellValueFast = chunkAtCell->getCellValueFast(num7);
                    int32_t num9 = Terrain::ExtractContents(cellValueFast);
                    if (num9 != 0)
                    {
                        Block *block = GameSingleton::blocksManager->blocks[num9];
                        if (block->shouldAvoid(cellValueFast))
                        {
                            return true;
                        }
                        if (block->IsCollidable)
                        {
                            Vector3 v(i, num8, j);
                            auto &customCollisionBoxes = block->getCustomCollisionBoxes(cellValueFast);
                            for(const auto& boundingBox : customCollisionBoxes)
                            {
                                if (box.intersection(BoundingBox(v + boundingBox.Min, v + boundingBox.Max)))
                                {
                                    return true;
                                }
                            }
                        }
                    }
                    num8++;
                    num7++;
                }
            }
        }
        return false;
    }

} // namespace PocketSurvival
